#!/usr/bin/env python
"""
机械臂高级控制器节点

该节点作为一个高级任务协调器，用于执行一个预定义的机械臂操作流程。
它不负责底层的机械臂关节控制，而是：
1. 订阅 'arm_command' 话题以接收启动指令。
2. 接收到指令后，发布 'arm_action' 话题来触发一个预设的、
   底层的机械臂动作序列（例如，一个抓取水瓶的演示）。
3. 通过固定的延时等待该动作完成。
4. 动作完成后，通过TTS（文本转语音）发布成功信息。
5. 在语音播报后，发布导航指令，让机器人返回初始位置。
"""
import rospy
import subprocess
import time
from std_msgs.msg import Bool, Int32, String

class ArmController:
    """
    封装了机械臂任务协调逻辑的ROS节点类。
    """
    def __init__(self):
        """
        初始化机械臂控制器节点。
        """
        # 初始化ROS节点
        rospy.init_node('arm_controller', anonymous=True)
        
        # --- ROS Publishers & Subscribers ---
        # 发布器，用于播报语音
        self.tts_pub = rospy.Publisher('tts_text', String, queue_size=10, latch=True)
        # 发布器，用于触发底层的机械臂动作
        self.arm_demo_pub = rospy.Publisher('arm_action', String, queue_size=10, latch=True)
        # 发布器，用于在任务完成后发布导航指令
        self.nav_pub = rospy.Publisher('navigation_command', String, queue_size=10)
        
        # 订阅来自中央控制节点的机械臂指令
        rospy.Subscriber('arm_command', String, self.arm_command_callback)
        
        rospy.loginfo("机械臂控制器已初始化，等待指令...")
    
    def arm_command_callback(self, msg):
        """
        处理来自 'arm_command' 话题的指令。
        当收到 "true" 时，执行一个固定的"抓取-递送-返回"任务序列。
        """
        if msg.data.lower() == "true":
            rospy.loginfo("收到机械臂启动指令，开始执行预设任务流程。")
            
            # 1. 触发底层机械臂演示动作
            self.arm_demo_pub.publish("start")
            rospy.loginfo("已发布 'start' 到 arm_action 话题，触发底层机械臂动作。")
    
            # 2. 等待机械臂动作完成
            # 注意：这里使用固定时长的休眠来等待。这是一个简单的同步方法，
            # 在实际应用中，更稳健的方式是订阅一个来自底层动作的状态话题或使用Actionlib。
            wait_duration = 8
            rospy.loginfo(f"等待 {wait_duration} 秒，以待机械臂动作完成...")
            rospy.sleep(wait_duration)
            
            # 3. 发布动作完成的语音消息
            completion_message = "我已拾取水瓶，现在递给您。"
            self.tts_pub.publish(completion_message)
            rospy.loginfo(f"机械臂动作完成，发布TTS消息: '{completion_message}'")
            
            # 4. 等待TTS播报完毕
            tts_wait_duration = 4
            rospy.loginfo(f"等待 {tts_wait_duration} 秒，以待语音播报完成...")
            rospy.sleep(tts_wait_duration)
            
            # 5. 发送导航指令，让机器人返回302房间
            return_location = "302"
            self.nav_pub.publish(return_location)
            rospy.loginfo(f"任务完成，已发布导航指令返回: {return_location}")

def main():
    """
    主函数，初始化并运行ArmController节点。
    """
    try:
        controller = ArmController()
        rospy.spin() # 保持节点运行，等待回调
    except rospy.ROSInterruptException:
        rospy.loginfo("机械臂控制器节点关闭。")

if __name__ == '__main__':
    main()